
/*************
author:: libo
license:: MIT
*****************/
#include<tool/libcell_tools_kd_node.h>

#include<Mesh/libcell_iterator.h>
#define quote lib_cell_quote

static inline void swap(template_v** values,int left,int right)
{
    template_v* temp=values[left];
    values[left]=values[right];
    values[right]=temp;
}

static void sort_vertex(template_v** values,int left,int right,int dim)
{
    if(left>=right)
    {
        return;
    }
    int m,d=dim%values[0]->point_size; 
    m = left;  
    for (int i=left+1;i<=right;i++)
    {
        if(values[i]->point[d]<values[left]->point[d])  
        { 
            swap(values,++m,i);
        }  
    }   
    swap(values,left,m);  
    sort_vertex(values,left,m-1,d);  
    sort_vertex(values,m+1,right,d);
}

static KD_Node* create_kd_node(template_v** values,int size,int dim)
{
    if(size==0)
    {
        return NULL;
    }
    //printf("begin sort\n");
    sort_vertex(values,0,size-1,dim);
    //printf("end sort\n");
    int wei=size/2;
    int d=dim%values[wei]->point_size;

    for(;(wei-1)>=0;wei--)
    {
        if(values[wei]->point[d]!=values[wei-1]->point[d])
        {
            break;
        }
    }
    if(wei==0)
    {
        wei++;
    } 
    KD_Node* re=(KD_Node*)malloc(sizeof(KD_Node));
    KD_Node_init(re);
    re->dim=d; 
    re->value=values[wei]; 
      
    if(size==1)
    {
        //printf("here4 wei:%d\n",wei);
        re->value=values[0];
       // printf("here4 wei:%d\n",wei);
        return re;
    }
    
    re->left=create_kd_node(values,wei,dim+1);
    re->right=create_kd_node(values+wei,size-wei,dim+1);

    re->left->parent=re;
    re->right->parent=re;

     
    return re;
}
KD_Node* create_kd_tree(template_v** values,int size,int dim)
{
   KD_Node* re=create_kd_node(values,size,dim);
   return re;
}
static double two_vertex_distance(Vertex* v1,Vertex *v2)
{
    if(v1==NULL||v2==NULL)
    {
        printf("cuowu\n");
    }
    double re=0;
    for(int i=0;i<v1->point_size;i++)
    {   
        re+=(v1->point[i]-v2->point[i])*(v1->point[i]-v2->point[i]);
    } 
    re=SAFE_SQRT(re);
    return re; 
}

Node* kd_tree_find_nearest_sphere(Vertex* mu,double r,KD_Node* kn)
{
    if(kn==NULL)
    {
        return NULL;
    }
    Node* n=NULL; 
    KD_Node* knn=kn;
    int d=0,point_size=mu->point_size;
    while(knn!=NULL)
    {
       // printf("id:%d\n",knn->value->id);
        if(knn!=kn)
        {
            n=node_overlying(n,knn);
        }
        if(knn->value==NULL)
        {
            printf("节点的value为NULL\n");
            return NULL;
        }
        d=knn->dim%point_size;
        mu->point[d]>= knn->value->point[d]?(knn=knn->right):(knn=knn->left); 
    }
    int flag=0;
    Node* re=NULL; 
    if(n==NULL)
    {
        if(two_vertex_distance(mu,kn->value)<r)
        {
            re=node_overlying(re,kn->value);
        }    
        return re;
    }
    Node* nit=n;
    KD_Node* kn1=NULL;
    kn1=(KD_Node*)(nit->value);
    if(two_vertex_distance(mu,kn1->value)<r)
    {
        re=node_overlying(re,kn1->value);
    }
    Node* n1=NULL,*n2=NULL;
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {
        kn1=(KD_Node*)(nit->value);
        kn1=KD_BROTHER(kn1);
        n1=NULL;
        // if(kn1==NULL)
        // {
        //     continue;
        // }
        if(KD_IS_INTERSECT_WITH_SPHERE(mu,r,kn1->parent)==1)
        {
            n1=kd_tree_find_nearest_sphere(mu,r,kn1);
            if(n1!=NULL)
            {
                n2=node_reverse(re);
                if(re==NULL)
                {
                    re=n1;
                }
                else
                {
                    n2->Next=n1;
                    n1->Prev=n2;
                } 
            }
        } 
    }
    free_node(n);
    return re;
}
KD_Node*  kd_tree_insert_vertex(KD_Node* kn,template_v* v)
{
    if(kn==NULL)
    {
        KD_Node* re=(KD_Node*)malloc(sizeof(KD_Node));
        KD_Node_init(re);
        re->dim=0; 
        re->value=v; 
        return re;
    }
    KD_Node* knn=kn;
    int d=0,point_size=v->point_size;
    while(knn->left!=NULL)
    {
        d=knn->dim%point_size;
        v->point[d]>= knn->value->point[d]?(knn=knn->right):(knn=knn->left); 
    }
    d=(knn->dim)%point_size;
    knn->left=(KD_Node*)malloc(sizeof(KD_Node));
    KD_Node_init(knn->left);

    knn->left->dim=(d+1)%point_size; 
    
    knn->right=(KD_Node*)malloc(sizeof(KD_Node));
    KD_Node_init(knn->right);

    knn->right->dim=(d+1)%point_size; 
    if(v->point[d]>=knn->value->point[d])
    {
        knn->left->value=knn->value;
        knn->right->value=v;
    }
    else
    {
        knn->left->value=v;
        knn->right->value=knn->value;
        knn->value=v; 
    }


    knn->left->parent=knn;
    knn->right->parent=knn;

    return kn;

}


static int is_existence_in_vs(Vertex* v,Vertex** vs,int size)
{
    int n=size-1;
    while(n>=0)
    {
        if(v->id==vs[n]->id)
        {
            return 1;
        }
        n--;
    }
    return 0;
}

typedef struct My_KDVertex{
template_v* v;
double distance;

}My_KDVertex;
static inline void my_kdvertex_init(My_KDVertex* mkdv)
{
    mkdv->v=NULL;
    mkdv->distance=0;
}
typedef struct My_KDVertexArray{
    int size,len;
    My_KDVertex** array;
}My_KDVertexArray;
static inline void my_kdvertexarray_init(My_KDVertexArray* mkdva,int len)
{
    mkdva->len=len;
    mkdva->size=0;
    mkdva->array=(My_KDVertex**)malloc(sizeof(My_KDVertex*)*len);
    memset(mkdva->array,0,sizeof(My_KDVertex*)*len);
}

static inline  void my_kdvertexarray_free(My_KDVertexArray*mkdva)
{
    if(mkdva==NULL)
    {
        return;
    }
    for(int i=0;i<mkdva->len;i++)
    {
        if(mkdva->array[i]==NULL)
        {break;}
        else
        {free(mkdva->array[i]);mkdva->array[i]=NULL;}
    } 
    free(mkdva->array);
    free(mkdva);
}


static inline void swap1(My_KDVertex** values,int left,int right)
{
    My_KDVertex* temp=values[left];
    values[left]=values[right];
    values[right]=temp;
}

static void sort_kdvertex(My_KDVertex** values,int left,int right)
{
    if(left>=right)
    {
        return;
    }
    int m; 
    m = left;  
    for (int i=left+1;i<=right;i++)
    {
        if(values[i]->distance<values[left]->distance)  
        { 
            swap1(values,++m,i);
        }  
    }   
    swap1(values,left,m);  
    sort_kdvertex(values,left,m-1);  
    sort_kdvertex(values,m+1,right);
}

static inline int  my_kd_tree_array_insert(My_KDVertexArray*mkdva,template_v* v,double distance)
{
    if(mkdva->size>=mkdva->len)
    {
        if(distance>=mkdva->array[mkdva->len-1]->distance)
        {
            return 0;
        } 
        else
        {
            My_KDVertex* new_mkdv=mkdva->array[mkdva->len-1];
            new_mkdv->v=v;new_mkdv->distance=distance;
            sort_kdvertex(mkdva->array,0,mkdva->len-1); 
        }
    } 
    else
    {
        mkdva->size++;
        My_KDVertex* new_mkdv=(My_KDVertex*)malloc(sizeof(My_KDVertex));
        my_kdvertex_init(new_mkdv);
        new_mkdv->v=v;new_mkdv->distance=distance;   
        mkdva->array[mkdva->size-1]=new_mkdv;
        sort_kdvertex(mkdva->array,0,mkdva->size-1); 
    }
    return 1; 
}

static inline double get_array_max_distance(My_KDVertexArray*mkdva)
{
    return mkdva->array[mkdva->size-1]->distance;
}

static void kd_tree_find_n_adjacent_vertices1(Vertex* mu,KD_Node*kn,int size,My_KDVertexArray*re)
{
    if(kn==NULL||size<=0)
    {return ;} 
    Node* n=NULL; 
    KD_Node* knn=kn;
    int d=0;
    while(knn!=NULL)
    {
        if(knn!=kn)
        {
            n=node_overlying(n,knn);
        } 
        d=knn->dim;
        mu->point[d]>=knn->value->point[d]?(knn=knn->right):(knn=knn->left);
    }
    if(n==NULL)
    {
        my_kd_tree_array_insert(re,kn->value,two_vertex_distance(kn->value,mu));
        return ;
    }
    KD_Node*kn1=NULL,*kn2=NULL;
    Node* nit=n;
    kn1=(KD_Node*)(nit->value);
    my_kd_tree_array_insert(re,kn1->value,two_vertex_distance(kn1->value,mu));
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {
        kn1=(KD_Node*)(nit->value);
        kn1=KD_BROTHER(kn1);
        if(re->size<size||KD_IS_INTERSECT_WITH_SPHERE(mu,get_array_max_distance(re),kn1->parent)==1)
        {
            kd_tree_find_n_adjacent_vertices1(mu,kn1,size,re); 
        } 
    } 
    free_node(n);
}

Node* kd_tree_find_n_adjacent_verticesn(Vertex* mu,KD_Node*kn,int n)
{
    My_KDVertexArray*mkdva=(My_KDVertexArray*)malloc(sizeof(My_KDVertexArray));
    my_kdvertexarray_init(mkdva,n);
    kd_tree_find_n_adjacent_vertices1(mu,kn,n,mkdva);
    Node* re=NULL;
    for(int i=0;i<mkdva->size;i++)
    {
        re=node_overlying(re,mkdva->array[i]->v);

    } 
    my_kdvertexarray_free(mkdva);
    return re;

}
Node* kd_tree_find_n_adjacent_vertices(Vertex * mu,KD_Node* kn,int n)
{
    if(kn==NULL)
    {return NULL;}
    Node* re=NULL;
    template_v** vs=(template_v**)malloc(sizeof(template_v*)*n);
    for(int i=0;i<n;i++)
    {
        KD_Node* kdn=kd_tree_find_nearest_and_except_vs(mu,kn,vs,i); 
        vs[i]=kdn->value;  
        re=node_overlying(re,vs[i]);
    } 
    free(vs);
    return re;
}

//寻找排除vs点集且离mu最近的点
//********************
KD_Node* kd_tree_find_nearest_and_except_vs(Vertex* mu,KD_Node* kn,Vertex** vs,int size)
{
    if(kn==NULL)
    {
        return NULL;
    }
    Node* n=NULL; 
    KD_Node* knn=kn;
    int d=0;
    while(knn!=NULL)
    {
       // printf("id:%d\n",knn->value->id);
        if(knn!=kn)
        {
            n=node_overlying(n,knn);
        }
        
        d=knn->dim;
        mu->point[d]>=knn->value->point[d]?(knn=knn->right):(knn=knn->left);
    }
    if(n==NULL)
    {
        return ((is_existence_in_vs(kn->value,vs,size)==1)?NULL:kn);
    }
    double distance1=0,distance2=0;
    KD_Node*re=NULL,*kn1=NULL,*kn2=NULL;
    Node* nit=n;
    re=(KD_Node*)(nit->value);
    is_existence_in_vs(re->value,vs,size)==1?(distance1=-1):(distance1=two_vertex_distance(re->value,mu));
    //nit=(Node*)(nit->Next);
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {
        kn1=(KD_Node*)(nit->value);
        kn1=KD_BROTHER(kn1);
       
        if(KD_IS_INTERSECT_WITH_SPHERE(mu,distance1,kn1->parent)==1)
        {
            kn2=kd_tree_find_nearest_and_except_vs(mu,kn1,vs,size);
            if(kn2!=NULL)
            {
                distance2=two_vertex_distance(mu,kn2->value);
                if(RELATIONSHIP_PARTIAL_ORDER(distance1,distance2)==1)
                {
                    distance1=distance2;
                    re=kn2;
                }
            } 
        }
    }
    free_node(n);
    return (distance1<0?NULL:re);
}
KD_Node* kd_tree_find_nearest(Vertex* mu,KD_Node* kn)
{
    if(kn==NULL)
    {
        return NULL;
    }
    Node* n=NULL; 
    KD_Node* knn=kn;
    int d=0,point_size=mu->point_size;
    while(knn!=NULL)
    {
       // printf("id:%d\n",knn->value->id);
        if(knn!=kn)
        {
            n=node_overlying(n,knn);
        }
        
        d=knn->dim%point_size;
        mu->point[d]>= knn->value->point[d]?(knn=knn->right):(knn=knn->left);
    }
    if(n==NULL)
    {
        return kn;
    }
    double distance1=0,distance2=0;
    KD_Node*re=NULL,*kn1=NULL,*kn2=NULL;
    Node* nit=n;
    re=(KD_Node*)(nit->value);
    distance1=two_vertex_distance(re->value,mu);
  //  nit=(Node*)(nit->Next);
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {
        kn1=(KD_Node*)(nit->value);
        kn1=KD_BROTHER(kn1);
       
        if(KD_IS_INTERSECT_WITH_SPHERE(mu,distance1,kn1->parent)==1)
        {
            kn2=kd_tree_find_nearest(mu,kn1);
            distance2=two_vertex_distance(mu,kn2->value);
            if(RELATIONSHIP_PARTIAL_ORDER(distance1,distance2)==1)
            {
                distance1=distance2;
                re=kn2;
            }
        }
    }
    free_node(n);
    return re;

}

int kd_node_size(KD_Node* kn)
{
    if(kn==NULL)
    {
        return 0;
    }
    if(kn->left==NULL&& kn->right==NULL)
    {
        return 1;
    }
    int re=0;
    re=re+kd_node_size(kn->left);
    re=re+kd_node_size(kn->right);

    return re; 

}
void free_kdnode(KD_Node* kn)
{
    if(kn==NULL)
    {
        return;
    }
    free_kdnode(kn->left);
    free_kdnode(kn->right);
    free(kn);
}

KD_Node* create_kd_tree_from_node(Node* node_v)
{
    if(node_v==NULL){return NULL;}
    
    template_v** values=(template_v**)malloc(sizeof(template_v*)*node_size(node_v));
    int i=0;
    for(Node* nit=node_v;nit!=NULL;nit=(Node*)(nit->Next))
    {
        values[i]=(template_v*)(nit->value); 
        i++;
    } 

    KD_Node* kn=create_kd_node(values,i,0);
    free(values);
    
    return kn;  
}

KD_Node*  create_kd_tree_from_mesh(Mesh * m)
{ 
    if(m->num_v(m)<=0)
    {
        return NULL;
    }
    template_v** values=(template_v**)malloc(sizeof(template_v*)*m->num_v(m));
    int i=0;
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {

        values[i]=(template_v*)(quote(vit)); 
        i++;
    } 
    //printf("begin create kd_ tree\n"); 
    KD_Node* kn=create_kd_node(values,i,0);
    free(values);
    
    return kn; 
}
// KD_Node* create_kd_tree_from_RBTree(std::map<int,template_v*> &mp)
// {
//     if(mp.size()<=0)
//     {
//         return NULL;
//     }
//     template_v** values=(template_v**)malloc(sizeof(template_v*)*mp.size());
//     int i=0;
//     for(auto vit=mp.begin();vit!=mp.end();vit++)
//     {
//         values[i]=vit->second;
//         i++;
//     }
//     KD_Node* kn=create_kd_node(values,i,0);
//     free(values);
//     return kn;
// }
KD_Node* create_kd_tree_from_RBTree(Int_RB_Tree* mp)
{
    if(mp->size<=0)
    {
        return NULL;
    }
    template_v** values=(template_v**)malloc(sizeof(template_v*)*mp->size);
    int i=0;
    for(auto vit=mp->begin(mp);vit.it!=NULL;vit++)
    {
        values[i]=(template_v*)(vit.second);
        i++;
    }
    KD_Node* kn=create_kd_node(values,i,0);
    free(values);
    return kn;


}
#undef quote 
